
#include "ModelReaderXML.h"
#include "../XMLTools.h"

#include <algorithm>
#include <fstream>
#include <filesystem>

#include "../RapidXML/rapidxml.hpp"

using std::cout;
using std::endl;

namespace MMM
{

    ModelReaderXML::ModelReaderXML()
        = default;

    ModelPtr ModelReaderXML::loadModel(const std::string& xmlFile, const std::string& name)
    {

        // load file
        std::ifstream in(xmlFile.c_str());

        if (!in.is_open())
        {
            MMM_ERROR << "Could not open XML file:" << xmlFile << endl;
            return ModelPtr();
        }

        std::stringstream buffer;
        buffer << in.rdbuf();
        std::string modelXML(buffer.str());
        in.close();

        std::filesystem::path filenameBaseComplete(xmlFile);
        std::filesystem::path filenameBasePath = filenameBaseComplete.parent_path();
        std::string basePath = filenameBasePath.string();

        ModelPtr res = createModelFromString(modelXML, basePath, name);
        if (!res)
        {
            MMM_ERROR << "Error while parsing file " << xmlFile << endl;
        }
        else
        {
            res->filename = xmlFile;
        }

        return res;
    }

    ModelPtr ModelReaderXML::createModelFromString(const std::string& xmlString, const std::string& basePath, const std::string& name)
    {
        ModelPtr model(new Model());

        bool modelOK = false;

        try
        {
            rapidxml::xml_document<char> doc;   // character type defaults to char
            char* y = doc.allocate_string(xmlString.c_str());
            doc.parse<0>(y);                    // 0 means default parse flags

            rapidxml::xml_node<>* node = doc.first_node();
            while (node)
            {
                std::string nodeName = XML::toLowerCase(node->name());
                if (nodeName == "robot")
                {
                    if (modelOK)
                    {
                        MMM_ERROR << "Multiple models in one file not supported!" << endl;
                    }
                    else
                    {
                        modelOK = processRobotTag(node, model, basePath);
                    }
                }
                else
                {
                    MMM_INFO << "Ignoring unknown XML tag <" << nodeName << ">" << endl;
                }
                node = node->next_sibling();
            }
        }
        catch (rapidxml::parse_error& e)
        {
            MMM_ERROR << "Could not parse data in XML definition" << endl
                      << "Error message:" << e.what() << endl
                      << "Position: " << endl << e.where<char>() << endl;
            return ModelPtr();
        }
        catch (std::exception& e)
        {
            MMM_ERROR << "Error while parsing XML definition" << endl
                      << "Error code:" << e.what() << endl;
            return ModelPtr();
        }
        catch (...)
        {
            cout << "Error while parsing XML definition" << endl;
            return ModelPtr();
        }

        if (modelOK)
        {
            if (name.empty())
            {
                model->setName(model->getType());
            }
            else
            {
                model->setName(name);
            }
            return model;
        }
        return ModelPtr();
    }


    bool ModelReaderXML::processRobotTag(rapidxml::xml_node<char>* tag, ModelPtr model, const std::string& basePath)
    {
        if (!tag)
        {
            return false;
        }
        if (!model)
        {
            return false;
        }

        //ex: <Robot Type="Armar3" RootNode="Armar3_Base">
        //ex: <Robot Type="ArmarIII Head" RootNode="Head Base"> TODO: how to multiple robots???

        // get name
        //rapidxml::xml_attribute<> *attr = tag->first_attribute("name", 0, false);
        rapidxml::xml_attribute<>* attr = tag->first_attribute("type", 0, false);
        if (!attr)
        {
            MMM_ERROR << "Robot tag: missing attribute 'type'" << endl;
            return false;
        }
        std::string jn = attr->value();
        if (jn.empty())
        {
            MMM_ERROR << "Robot type tag: null string" << endl;
            return false;
        }
        model->setType(jn);

        // get root
        attr = tag->first_attribute("rootnode", 0, false);
        if (!attr)
        {
            MMM_ERROR << "Robot tag: missing attribute 'rootnode'" << endl;
            return false;
        }
        std::string rn = attr->value();
        if (rn.empty())
        {
            MMM_ERROR << "Robot RootNode tag: null string" << endl;
            return false;
        }
        model->setRoot(rn);


        rapidxml::xml_node<>* node = tag->first_node();
        while (node)
        {
            std::string nodeName = XML::toLowerCase(node->name());
            if (nodeName == "robotnode")
            {
                processRobotNodeTag(node, model, basePath);
            }
            else if (nodeName == "robotnodeset")
            {
                //cout << "Process robotnodeset tag" << endl;
                processRobotNodeSetTag(node, model, basePath);
            }
            else if (nodeName == "endeffector")
            {
                // ignore
            }
            else
            {
                MMM_INFO << "Ignoring unknown XML tag <" << nodeName << ">" << endl;
            }
            node = node->next_sibling();
        }
        return true;
    }


    bool ModelReaderXML::processRobotNodeTag(rapidxml::xml_node<char>* tag, ModelPtr model, const std::string& basePath)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL RobotNodeTag tag in XML definition" << endl;
            return false;
        }
        if (!model)
        {
            MMM_ERROR << "NULL model?!" << endl;
            return false;
        }

        // get name
        rapidxml::xml_attribute<>* attr = tag->first_attribute("name", 0, false);
        if (!attr)
        {
            MMM_ERROR << "RobotNode tag: missing attribute 'name'" << endl;
            return false;
        }
        std::string jn = attr->value();
        if (jn.empty())
        {
            MMM_ERROR << "RobotNode tag: null name string" << endl;
            return false;
        }

        MMM::ModelNodePtr modelNode(new ModelNode());
        modelNode->name = jn;

        rapidxml::xml_node<>* node = tag->first_node();
        while (node)
        {
            std::string nodeName = XML::toLowerCase(node->name());
            if (nodeName == "transform")
            {
                // TRANSFORM node
                processTransformTag(node, modelNode);
            }
            else if (nodeName == "visualization")
            {
                rapidxml::xml_node<>* visuFileXMLNode = node->first_node("file", 0, false);
                if (visuFileXMLNode)
                {
                    attr = visuFileXMLNode->first_attribute("type", 0, false);
                    if (attr)
                    {
                        modelNode->segment.visuType = attr->value();
                    }
                    modelNode->segment.visuFile = XML::processFileNode(visuFileXMLNode, basePath);
                }
            }
            else if (nodeName == "collisionmodel")
            {
                // ignore
            }
            else if (nodeName == "childfromrobot")
            {
                //          MMM_ERROR << "Warning: Ignoring ChildFromRobot Tag: Ensure that the whole robot is defined in one file!" << endl;
                // CHILDFROMROBOT node
                processChildFromRobotTag(node, modelNode, model, basePath);
            }
            else if (nodeName == "physics")
            {
                // PHYSICS node
                processPhysicsTag(node, modelNode);
            }
            else if (nodeName == "child")
            {
                // CHILD node
                processChildTag(node, modelNode);
            }
            else if (nodeName == "joint")
            {
                // JOINT node
                processJointTag(node, modelNode);
            }
            else if (nodeName == "sensor")
            {
                // SENSOR/MARKER node
                processSensorTag(node, modelNode);
            }
            else
            {
                MMM_INFO << "Ignoring unknown XML tag <" << nodeName << ">" << endl;
            }
            node = node->next_sibling();
        }

        model->addModelNode(modelNode);

        return true;
    }


    bool ModelReaderXML::processChildFromRobotTag(
        rapidxml::xml_node<char>* tag, ModelNodePtr modelNode,
        ModelPtr model, const std::string& basePath
    )
    {
        MMM_INFO << "X: processing ChildFromRobot Tag" << endl;

        /*
        <Robot Type="Armar3" RootNode="Armar3_Base">
            <RobotNode name="TrafoToHead">
                <Transform>
                    <Translation x="0" y="0" z="118"/>
                </Transform>
                <Visualization enable="true">
                    <CoordinateAxis type="Inventor" enable="false" scaling="8"/>
                </Visualization>

                <ChildFromRobot>
                    <File importEEF="true">ArmarIII-Head.xml</File>
                </ChildFromRobot>
            </RobotNode>
            ...
        </Robot>

        ... ArmarIII-LeftArm.xml ...
        <Robot Type="ArmarIII Head" RootNode="Head Base">
        */

        if (!tag)
        {
            MMM_ERROR << "NULL ChildFromRobot tag in XML definition" << endl;
            return false;
        }
        if (!modelNode)
        {
            MMM_ERROR << "NULL modelNode?!" << endl;
            return false;
        }

        // get file node and get file name from it
        std::string fileName;
        rapidxml::xml_node<>* fileXMLNode = tag->first_node("file", 0, false);
        if (!fileXMLNode)
        {
            MMM_ERROR << "NULL fileXMLNode?!" << endl;
            return false;
        }
        fileName = XML::processFileNode(fileXMLNode, basePath);

        // load model "child" from file
        ModelReaderXMLPtr mr(new ModelReaderXML());
        ModelPtr modelChild = mr->loadModel(fileName);
        if (!modelChild)
        {
            MMM_ERROR << "ModelReaderXML.processChildFromRobotTag: Could not load model from file " << fileName << endl;
            return false;
        }
        else
        {
            //for each node in modelChild append to model
            std::vector<ModelNodePtr> modelChildModelNodes = modelChild->getModels();
            for (auto& modelChildModelNode : modelChildModelNodes)
            {
                model->addModelNode(modelChildModelNode);
            }

            // add modelchild.rootNode as child in modelnode to keep track of sub-tree references
            modelNode->children.push_back(modelChild->rootNode);
        }
        return true;
    }


    bool ModelReaderXML::processChildTag(rapidxml::xml_node<char>* tag, ModelNodePtr modelNode)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL Child tag in XML definition" << endl;
            return false;
        }
        if (!modelNode)
        {
            MMM_ERROR << "NULL modelNode?!" << endl;
            return false;
        }

        rapidxml::xml_attribute<>* attr = tag->first_attribute("name", 0, false);
        if (!attr)
        {
            MMM_ERROR << "Child tag: missing attribute 'name'" << endl;
            return false;
        }
        std::string jn = attr->value();
        if (jn.empty())
        {
            MMM_ERROR << "Child tag: null name string" << endl;
            return false;
        }
        modelNode->children.push_back(jn);
        return true;
    }

    bool ModelReaderXML::processSensorTag(rapidxml::xml_node<char>* tag, ModelNodePtr modelNode)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL sensor tag in XML definition" << endl;
            return false;
        }
        rapidxml::xml_attribute<>* attr;
        std::string sensorType;

        attr = tag->first_attribute("type", 0, false);
        if (attr)
        {
            sensorType = XML::toLowerCase(attr->value());
        }
        else
        {
            cout << "No 'type' attribute for <Sensor> tag. Skipping Sensor definition of node " << modelNode->name << "!" << endl;
            return false;
        }

        if (sensorType != "position")
        {
            cout << "Skipping unknown sensor type: " << sensorType << endl;
            return false;
        }

        // get name
        attr = tag->first_attribute("name", 0, false);
        if (!attr)
        {
            MMM_ERROR << "Sensor tag: missing attribute 'name'" << endl;
            return false;
        }
        std::string sensorName = attr->value();
        if (sensorName.empty())
        {
            MMM_ERROR << "Sensor tag: null name string" << endl;
            return false;
        }

        Eigen::Matrix4f localTransform = Eigen::Matrix4f::Identity();

        rapidxml::xml_node<>* node = tag->first_node();

        while (node)
        {
            std::string nodeName = XML::toLowerCase(node->name());
            if (nodeName == "transform")
            {
                localTransform = XML::processTransformTag(node);
            }
            else
            {
                cout << "Skipping unknown xml tag in sensor definition:" << nodeName << endl;
            }
            node = node->next_sibling();
        }

        MarkerInfoPtr s(new MarkerInfo());
        s->name = sensorName;
        s->localTransform = localTransform;

        modelNode->markers.push_back(s);
        return true;
    }


    bool ModelReaderXML::processJointTag(rapidxml::xml_node<char>* tag, ModelNodePtr modelNode)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL joint tag in XML definition" << endl;
            return false;
        }
        if (!modelNode)
        {
            MMM_ERROR << "NULL modelNode?!" << endl;
            return false;
        }
        std::string jointType;
        rapidxml::xml_attribute<>* attr = tag->first_attribute("type", 0, false);
        if (attr)
        {
            jointType = XML::toLowerCase(attr->value());
        }
        else
        {
            MMM_INFO << "No 'type' attribute for <Joint> tag. Assuming fixed joint!" << endl;
            jointType = "fixed";
        }


        attr = tag->first_attribute("offset", 0, false);
        if (attr)
        {
            modelNode->joint.offset = XML::convertToFloat(attr->value());
        }

        rapidxml::xml_node<>* node = tag->first_node();

        modelNode->joint.initValue = 0.0f;
        // init without limit
        modelNode->joint.limitLo = float(-M_PI);
        modelNode->joint.limitHi = float(M_PI);
        modelNode->joint.limitUnits = MMM::eRadian;

        //float initvalue = 0.0f;

        Eigen::Vector3f axis;
        axis << 0, 0, 1.0f;

        while (node)
        {
            std::string nodeName = XML::toLowerCase(node->name());
            if (nodeName == "limits")
            {
                float lo, hi;
                processLimitsNode(node, lo, hi);
                modelNode->joint.limitLo = lo;
                modelNode->joint.limitHi = hi;
                if (jointType == "revolute")
                {
                    modelNode->joint.limitUnits = MMM::eRadian;
                }
                else
                {
                    // TODO: read this from xml file
                    modelNode->joint.limitUnits = MMM::eMM;
                }
            }
            else if (nodeName == "axis")
            {
                if (jointType == "revolute" || jointType == "prismatic")
                {
                    axis[0] = XML::getFloatByAttributeName(node, "x");
                    axis[1] = XML::getFloatByAttributeName(node, "y");
                    axis[2] = XML::getFloatByAttributeName(node, "z");
                }
                else
                {
                    MMM_WARNING << "Axis tag must be within a revolute/prismatic joint" << endl;
                }
            }
            else if (nodeName == "translationdirection")
            {
                //ignore
            }
            else if (nodeName == "offset")
            {
                modelNode->joint.offset = XML::getFloatByAttributeName(node, "value");
            }
            else if (nodeName == "init")
            {
                // this is the initial value for prismatic joints... should also work for revolute joints
                modelNode->joint.initValue = XML::getFloatByAttributeName(node, "value");
            }
            else if (nodeName == "postjointtransform")
            {
                // ignore
            }
            else if (nodeName == "maxvelocity")
            {
                modelNode->joint.maxVelocity = XML::getFloatByAttributeName(node, "value");
                // assuming m/s

            }
            else if (nodeName == "maxacceleration")
            {
                modelNode->joint.maxAcceleration = XML::getFloatByAttributeName(node, "value");
                // assuming m/s^2

            }
            else if (nodeName == "maxtorque")
            {
                modelNode->joint.maxTorque = XML::getFloatByAttributeName(node, "value");
                // assuming Nm
            }
            else if (nodeName == "propagatejointvalue")
            {
                rapidxml::xml_attribute<>* attrPropa;
                attrPropa = node->first_attribute("name", 0, false);
                if (!attrPropa)
                {
                    MMM_ERROR << "Expecting 'name' attribute in <PropagateJointValue> tag..." << endl;
                }
                else
                {
                    std::string s(attrPropa->value());
                    float f = 1.0f;
                    attrPropa = node->first_attribute("factor", 0, false);
                    if (attrPropa)
                    {
                        f = XML::convertToFloat(attrPropa->value());
                    }
                    modelNode->joint.propagateJointValues[s] = f;
                }
            }
            else
            {
                MMM_WARNING << "XML definition <" << nodeName << "> not supported" << endl;
            }

            node = node->next_sibling();
        }

        if (jointType == "revolute")
        {
            modelNode->joint.jointType = eRevolute;
            modelNode->joint.axis = axis;
        }
        else if (jointType == "prismatic")
        {
            modelNode->joint.jointType = ePrismatic;
            modelNode->joint.axis = axis;
        }
        else
        {
            modelNode->joint.jointType = eFixed;
        }
        return true;
    }


    bool ModelReaderXML::processTransformTag(rapidxml::xml_node<char>* tag, ModelNodePtr modelNode)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL Child tag in XML definition" << endl;
            return false;
        }
        if (!modelNode)
        {
            MMM_ERROR << "NULL modelNode?!" << endl;
            return false;
        }
        Eigen::Matrix4f m = XML::processTransformTag(tag);
        modelNode->localTransformation = m;
        return true;
    }


    bool ModelReaderXML::processPhysicsTag(rapidxml::xml_node<char>* tag, ModelNodePtr modelNode)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL physics tag in XML definition" << endl;
            return false;
        }
        if (!modelNode)
        {
            MMM_ERROR << "NULL modelNode?!" << endl;
            return false;
        }

        rapidxml::xml_node<>* massXMLNode = tag->first_node("mass", 0, false);
        if (massXMLNode)
        {
            modelNode->segment.mass = XML::getFloatByAttributeName(massXMLNode, "value");
            float scaling = XML::getUnitsScalingToKG(massXMLNode);
            modelNode->segment.mass *= scaling;
        }
        else
        {
            MMM_ERROR << "Expecting mass tag for physics node in <" << tag->name() << ">." << endl;
        }
        rapidxml::xml_node<>* comXMLNode = tag->first_node("com", 0, false);
        if (comXMLNode)
        {
            /*attr =*/ comXMLNode->first_attribute("location", 0, false);
            // ignoring location attribute

            modelNode->segment.com(0) = XML::getOptionalFloatByAttributeName(comXMLNode, "x", 0.0f);
            modelNode->segment.com(1) = XML::getOptionalFloatByAttributeName(comXMLNode, "y", 0.0f);
            modelNode->segment.com(2) = XML::getOptionalFloatByAttributeName(comXMLNode, "z", 0.0f);

            float scaling = XML::getUnitsScalingToMeter(comXMLNode);
            modelNode->segment.com *= scaling;
        }
        rapidxml::xml_node<>* inMatXMLNode = tag->first_node("inertiamatrix", 0, false);
        if (inMatXMLNode)
        {
            modelNode->segment.inertia = XML::process3x3Matrix(inMatXMLNode);
            /*float scalingWeight =*/ XML::getUnitsScalingToKG(inMatXMLNode);
            /*float scalingLength =*/ XML::getUnitsScalingToMeter(inMatXMLNode);
            // inertia matrices are defined with kg*m^2, so we have to adapt the scaling
            // float scaling = scalingWeight * scalingLength*scalingLength;
            float scaling = 1.0f;
            modelNode->segment.inertia *= scaling;

        }
        else
        {
            modelNode->segment.inertia.setZero();
        }
        rapidxml::xml_node<>* ignoreColXMLNode = tag->first_node("ignorecollision", 0, false);
        while (ignoreColXMLNode)
        {
            rapidxml::xml_attribute<>* attr = ignoreColXMLNode->first_attribute("name", 0, false);
            if (!attr)
            {
                MMM_ERROR << "Expecting 'name' attribute in <IgnoreCollision> tag..." << endl;
            }
            else
            {
                std::string s(attr->value());
                modelNode->segment.ignoreCollisions.push_back(s);
            }
            ignoreColXMLNode = ignoreColXMLNode->next_sibling("ignorecollision", 0, false);
        }
        /*rapidxml::xml_node<> *simulationtype =*/ tag->first_node("simulationtype", 0, false);
        // ignored

        return true;
    }


    bool ModelReaderXML::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi)
    {
        if (!limitsXMLNode)
        {
            MMM_ERROR << "NULL data for limitsXMLNode in processLimitsNode()" << endl;
            jointLimitLo = jointLimitHi = 0.0f;
            return false;
        }

        jointLimitLo = XML::getFloatByAttributeName(limitsXMLNode, "lo");
        jointLimitHi = XML::getFloatByAttributeName(limitsXMLNode, "hi");
        float scaling = XML::getUnitsScalingToRadian(limitsXMLNode);
        jointLimitLo *= scaling;
        jointLimitHi *= scaling;
        return true;
    }


    bool ModelReaderXML::processRobotNodeSetTag(rapidxml::xml_node<char>* tag, ModelPtr model, const std::string& basePath)
    {
        if (!tag)
        {
            MMM_ERROR << "NULL RobotNodeSetTag tag in XML definition" << endl;
            return false;
        }
        if (!model)
        {
            MMM_ERROR << "NULL model?!" << endl;
            return false;
        }

        // get name
        rapidxml::xml_attribute<>* attr = tag->first_attribute("name", 0, false);
        if (!attr)
        {
            MMM_ERROR << "RobotNodeSet tag: missing attribute 'name'" << endl;
            return false;
        }
        std::string jn = attr->value();
        if (jn.empty())
        {
            MMM_ERROR << "RobotNodeSet tag: null name string" << endl;
            return false;
        }

        ModelNodeSet mns = ModelNodeSet();
        mns.ModelNodeSetName = jn;


        attr = tag->first_attribute("kinematicroot", 0, false);
        if (attr)
        {
            mns.rootName = attr->value();
        }
        attr = tag->first_attribute("tcp", 0, false);
        if (attr)
        {
            mns.tcpName = attr->value();
        }


        rapidxml::xml_node<>* node = tag->first_node();

        while (node)
        {
            mns.ModelNodes.push_back(XML::getStringByAttributeName(node, "name"));
            node = node->next_sibling();
        }

        model->addModelNodeSet(mns);

        return true;
    }


}
